%==========================================================================
                        %Plot Results
%==========================================================================

disp('Final Values:') %  final values
fprintf('CdL = %5.3f\n',CdL_flt)
fprintf('Cr  = %5.4f\n',Fr_flt/mass/9.81)

%% constant speed classier
figure(1) 
hold on
plot(Rec(:,1),Rec(:,2))
plot(Rec(:,1),Rec(:,9)*40,'.')
ylabel('speed mph')
xlabel('time')
legend('vehicle speed','constant speed')
title('Constant Speed Classifier')

%% lateral classifer
figure(2) 
hold on
plot(Rec(:,1),Rec(:,8))
plot(Rec(:,1),Rec(:,11),'linewidth',2)
plot(Rec(:,1),Rec(:,10))
ylabel('lateral acceleration (m/s)^2')
xlabel('time')
legend('lateral acceleration','average later accel','lateral motion')
title('Lateral Motion Classifier')
   
%% data records
figure(3) 
scatter(Rec2(:,2),Rec2(:,1))
hold on
plot([0,1000],[Fr_flt,Fr_flt+CdL_flt*1000],'--','linewidth',2)
ylabel('Total Drag (N)')
xlabel('speed^2 (m/s)^2')
legend('measurements','fitted line')
title('Measurements')
axis([0 1000 0 4000])

%% Estimates
figure(4) 
subplot(2,1,1)
plot(Rec(:,14))
hold on
plot([0,size(Rec,1)],[2.61, 2.61])
title('Estimates')
ylabel('CdL')
legend('estimates','reference')
axis([0 size(Rec,1) 0 5])
subplot(2,1,2)
plot(Rec(:,15)/9.81/mass)
hold on
plot([0,size(Rec,1)],[0.007, 0.007])
axis([0 size(Rec,1) 0 0.03])
ylabel('Fr')
xlabel('time steps')

%% drag table
figure(5) 
n=1:1:table_size;
hold on
% sort according to weight
for m=1:table_size
    color(m) = (drag_table(m,2))/(max(drag_table(:,2))-min(drag_table(:,2)));
end
[~,index]=sort(drag_table(:,2));

% plot legend
for i=[1,table_size-5,table_size]
h2 = plot((index(i)-0.5)*(((1024-100)/table_size))+100,drag_table(index(i),1),'o');
h2.MarkerFaceColor = [1,1,1]-[50/255,1,1]*color(index(i));
h2.MarkerEdgeColor = [0.3,0.3,0.3] ;
end
plot([0,900],[x(2),900*x(1)+x(2)])
hLegend=legend(strcat('w = ',num2str(round(drag_table(index(1),2)))),...
    strcat('w = ',num2str(round(drag_table(index(15),2)))),...
    strcat('w = ',num2str(round(drag_table(index(table_size),2)))),'Fitted Line');

% scatter plot
for m=1:table_size
h2=plot((m-0.5)*(((1024-100)/table_size))+100,drag_table(m,1),'o');
h2.MarkerFaceColor = [1,1,1]-[50/255,1,1]*color(m);
h2.MarkerEdgeColor = [0.3,0.3,0.3] ;
h2.MarkerSize = 10;
end

xlabel('speed square (m/s)^2')
ylabel('total drag (N)')
axis([0 1000 0 4000])
title('Drag Table')




